#ifndef AS5600_H
#define AS5600_H

#include "Wire.h"
#include <Arduino.h>
#include <cstdint>

class Sensor_AS5600
{
public:
    Sensor_AS5600(int Mot_Num);
    void Sensor_init(TwoWire *_wire = &Wire);
    void Sensor_update();
    float getAngle();
    float getVelocity();
    float getMechanicalAngle();
    double getSensorAngle();

private:
    int _Mot_Num;
    float angle_prev = 0;
    long angle_prev_ts = 0;
    float vel_angle_prev = 0;
    long vel_angle_prev_ts = 0;
    int32_t full_rotations = 0; // number of full-rotations 
    int32_t vel_full_rotations = 0;
    Twowire *wire;
};

#endif